Skip to content

Move joint eef now can take multiple waypoints as input.#339

Open
matafela wants to merge 3 commits into
mainfrom
cj/move-joint-eef-multi-waypoint
Open

Move joint eef now can take multiple waypoints as input.#339
matafela wants to merge 3 commits into
mainfrom
cj/move-joint-eef-multi-waypoint

Conversation

@matafela

@matafela matafela commented Jun 29, 2026

Copy link
Copy Markdown
Collaborator

Description

Move joint/end_effector now can take multiple waypoints as input.

TODO:

  • Move joints.
  • Move EndEffector

Please include a summary of the change and which issue is fixed. Please also include relevant motivation and context.
List any dependencies that are required for this change.

Type of change

  • Enhancement (non-breaking change which improves an existing functionality)

Checklist

  • I have run the black . command to format the code base.
  • I have made corresponding changes to the documentation
  • I have added tests that prove my fix is effective or that my feature works
  • Dependencies have been updated, if applicable.

@matafela matafela marked this pull request as ready for review June 29, 2026 09:45
Copilot AI review requested due to automatic review settings June 29, 2026 09:45

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copilot was unable to review this pull request because the user who requested the review has reached their quota limit.

@matafela matafela requested a review from yuecideng June 29, 2026 09:45
@yuecideng

Copy link
Copy Markdown
Contributor

Reviewed the current implementation against main (7bcd2144..bb06b5ed). The change is narrowly scoped and the targeted atomic-action tests pass, but I think this needs fixes before merge.

Blocking / should fix

  1. Multi-waypoint trajectories do not guarantee exact visits to intermediate waypoints

    • embodichain/lab/sim/atomic_actions/trajectory.py:336
    • embodichain/lab/sim/atomic_actions/trajectory.py:360

    The implementation passes all keyframes to interpolate_with_distance() and resamples the entire path to a fixed count. That preserves path order, but it does not guarantee intermediate waypoints appear as emitted trajectory samples. For example, keyframes [0, 1, 3] with 5 samples become [0, 0.75, 1.5, 2.25, 3], so waypoint 1 is skipped. This conflicts with the new “visits every waypoint” API contract and can miss clearance/safety waypoints.

    Suggested fix: interpolate segment-by-segment while preserving each keyframe boundary, or explicitly insert exact keyframes into the sampled output. Also validate that sample_interval can accommodate all required keyframes.

  2. Empty multi-waypoint targets silently succeed as no-ops

    • embodichain/lab/sim/atomic_actions/trajectory.py:88
    • embodichain/lab/sim/atomic_actions/trajectory.py:162
    • embodichain/lab/sim/atomic_actions/actions.py:239

    (n_envs, 0, 4, 4) and (n_envs, 0, dof) pass validation. Planning then prepends only start_qpos, returning a successful stationary trajectory. These should be rejected with a clear error, and tests should cover empty waypoint tensors.

  3. EndEffectorPoseTarget is now ambiguous for Place

    • embodichain/lab/sim/atomic_actions/core.py:73
    • embodichain/lab/sim/atomic_actions/actions.py:687

    EndEffectorPoseTarget now documents the 4D multi-waypoint form, and Place accepts the same target type, but Place.execute() still expects a single pose and will fail later through a generic shape check. Either reject 4D targets in Place with a clear error, split the target type, or implement multi-waypoint place behavior.

  4. Tests mock out the behavior that most needs protection

    • tests/sim/atomic_actions/test_actions.py:174
    • tests/sim/atomic_actions/test_actions.py:287
    • tests/sim/atomic_actions/test_trajectory.py:252

    The new tests assert that keyframes are passed into the interpolator, but not that the returned trajectory actually preserves intermediate waypoints. Add a real interpolation regression test with a simple uneven path and assert intermediate waypoints are emitted.

Minor

  • Public docs still list only single-target shapes:
    • docs/source/overview/sim/atomic_actions/index.md:97
    • docs/source/overview/sim/atomic_actions/builtin_actions.md:29

Verification

Ran:

pytest -q tests/sim/atomic_actions/test_trajectory.py tests/sim/atomic_actions/test_actions.py

Result: 45 passed in 36.31s.

@yuecideng yuecideng left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please check the comments

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants